import os
import sys
sys.path.append("..")
import cv2
import numpy as np
import open3d as o3d
from utils.txt2intrinsic import txt2intrinsic


def visualize_rgbd(rgb_path, depth_path, intrinsic_mat):
    rgb = o3d.io.read_image(rgb_path)
    depth = o3d.io.read_image(depth_path)
    intrinsic = o3d.camera.PinholeCameraIntrinsic(1280, 720, intrinsic_mat[0, 0], intrinsic_mat[1, 1], intrinsic_mat[0, 2], intrinsic_mat[1, 2])

    rgbd_image = o3d.geometry.RGBDImage.create_from_color_and_depth(rgb, depth, convert_rgb_to_intensity=False, depth_trunc=10.0)
    pcd = o3d.geometry.PointCloud.create_from_rgbd_image(rgbd_image, intrinsic)
    o3d.io.write_point_cloud("./rgbd_pcd.ply", pcd)


if __name__ == "__main__":
    rgb_path = "/home/liuyun/HHO-dataset/data_processing/exp_data/20230421_data/rgbd/Color/vis_-ob_camera_01-color-image_raw_1682088954833670899.png"
    depth_path = "/home/liuyun/HHO-dataset/data_processing/exp_data/20230421_data/rgbd/Depth/vis_-ob_camera_01-depth-image_raw_1682088954834981645.png"
    intrinsic_path = "/home/liuyun/HHO-dataset/data_processing/exp_data/helmet/helmet1_AY3A131005M/intrinsic.txt"
    intrinsic_mat, _ = txt2intrinsic(intrinsic_path)
    visualize_rgbd(rgb_path, depth_path, intrinsic_mat)
